/*
 *  surveyorsvs.h
 *
 *  David Lobato 2011 <dav.lobato@gmail.com>
 *
 *  This file is part of Surveyor SVS player/stage driver.
 *
 *  Surveyor SVS player/stage driver is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *   Surveyor SVS player/stage driver is distributed in the hope that it will be useful,
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *   GNU General Public License for more details.
 *
 *   You should have received a copy of the GNU General Public License
 *   along with Surveyor SVS player/stage driver.
 *   If not, see <http://www.gnu.org/licenses/>.
 *
 */

#ifndef SURVEYORSVS_COMMS_H_
#define SURVEYORSVS_COMMS_H_

#ifdef __cplusplus
extern "C"
{
#endif

#include <limits.h>
#include <stddef.h>

/* command opcodes */
#define SVS_OPCODE_IMAGE_160x120 'a'
#define SVS_OPCODE_IMAGE_320x240 'b'
#define SVS_OPCODE_IMAGE_640x480 'c'
#define SVS_OPCODE_IMAGE_1280x1024 'A'
#define SVS_OPCODE_BATTERYSTAT 'D'
#define SVS_OPCODE_GRABIMG 'I'
#define SVS_OPCODE_MOTOR 'M'
#define SVS_OPCODE_STAT 'V'
#define SVS_OPCODE_SERVO1 'S'
#define SVS_OPCODE_SERVO2 's'
#define SVS_OPCODE_FAILSAFE 'F'

typedef enum {
	CMD_IMAGE_160x120,
	CMD_IMAGE_320x240,
	CMD_IMAGE_640x480,
	CMD_IMAGE_1280x1024,
	CMD_BATTERYSTAT,
	CMD_GRABIMG,
	CMD_MOTOR1,
	CMD_MOTOR2,
	CMD_STAT,
	CMD_SERVO1,
	CMD_SERVO2,
	CMD_FAILSAFE,
	CMD_RESET,
	CMD_MAX
} svs_command_enum; ///< SVS implemented commands

typedef enum {
	SVS_MASTER,//SRV-1 master module
	SVS_SLAVE,//SRV-1 slave module
	SVS_MODULE_MAX
} svs_module_enum;

typedef enum {
	BATTERY_OK,
	BATTERY_LOW,
} svs_battery_stat_enum; ///< Battery states

typedef enum {
	IMAGE_OFF, //initial value, can't be used to set image mode
	RGB888_160x120,
	RGB888_320x240,
	RGB888_640x480,
	RGB888_1280x1024,
	IMAGE_MODE_MAX
} svs_image_mode_enum; ///< Implemented image modes

typedef struct {
	svs_command_enum cmd_id;
	char* cmd;
	size_t cmd_size;
} svs_command_t;

typedef struct {
	svs_image_mode_enum mode_id;
	svs_command_enum cmd_id;
	unsigned int width;
	unsigned int height;
	unsigned int channels;
	unsigned int bpp;
} svs_image_mode_t; ///< Info related with the selected image mode


/* Communications */
#define SVS_COMM_KEEPALIVE 1
#define SVS_COMM_TIMEOUT_MS 2000

/* others */
#define _STR(exp) #exp
#define STRINGIZE(exp) _STR(exp)
#define LINE_INFO __FILE__"[" STRINGIZE( __LINE__ ) "]"
#define IP_ADDR_MAX_STR_LENGTH 16

typedef struct private_data_t private_data_t;

typedef struct
{
	int port;///< tcp ports
	int fd;///< fd if port is open. (-1 = not valid)
	svs_image_mode_t image_mode; ///<  Image mode. Set by the driver to the right values after svs_set_image_mode(...)
	size_t image_buf_size;///< Size for left and right image buffers
	unsigned char* image_buf;//< Image buffer

	//absolute max values
	float max_values_bank1[2];//0 left, 1 right
	float max_values_bank2[2];//0 left, 1 right

	private_data_t* priv;///< module private data
} svs_module_t;

/**
 * @brief Type definition that is used in the communication link between the Surveyor Driver implementation and the robot itself
 * @ingroup driver_surveyor
 */
typedef struct
{
	char ip_addr[IP_ADDR_MAX_STR_LENGTH]; ///< Ip addr to connect with the SVS system
	svs_module_t modules[SVS_MODULE_MAX];
} svs_comm_t;

/**
 * Creates a svs_comm_t instance. It doesn't open any connection \sa svs_open
 * \param ip_addr ip address string
 * \param master_port port assigned to master server
 * \param slave_port port assigned to slave server
 * \return an initialized svs_comm_t instance
 */
svs_comm_t* svs_create(const char *ip_addr, int master_port , int slave_port );

/**
 * Destroy a svs_comm_t instance, deallocating used memory.
 * \param r svs instance to destroy
 */
void svs_destroy(svs_comm_t* r);

/**
 * Init communications with SVS system
 * \param r svs instance to initialize
 * \return 0 if success, !=0 otherwise
 */
int svs_init(svs_comm_t* r);

/**
 * Reset SVS system sending $! command
 * \param r svs instance to reset
 * \return 0 if success, !=0 otherwise
 */
int svs_reset(svs_comm_t* r);

/**
 * Close connection with SVS system
 * \param r svs instance to close
 * \return 0 if success, !=0 otherwise
 */
int svs_close(svs_comm_t* r);


/**
 * Get battery status
 * \param r svs instance. Msg is sent to master module
 * \return Battery status or <0 if error
 */
svs_battery_stat_enum svs_get_battery_status(svs_comm_t* r);

/**
 * Sets image mode
 * \param module srv module
 * \return 0 if success, != 0 otherwise
 */
int svs_set_image_mode(svs_module_t* module, svs_image_mode_enum mode);

/**
 * Get an image from a svs instance. Memory is owned by the svs instance,
 * so just use it and let it go. Image buffer is r->image_buf_size[module] and format is r->image_mode[module]
 * \param module srv module
 * \return if success a pointer to the image buffer, NULL otherwise
 */
const unsigned char* svs_get_image(svs_module_t* module);

/**
 * Set motor1 bank speeds. Speeds are in the range [-1.0(max reverse)..1.0(max forward)]
 * \param module svs module to send the msg to
 * \param left left motor speed
 * \param right right motor speed
 * \param durationms speed command duration in ms
 * \return 0 if success, != 0 otherwise
 */
int svs_set_motor1_speeds(svs_module_t* module, float left, float right, int durationms );

/**
 * Set absolute max speeds for motor1 bank.
 * \param module
 * \param left absolute max value for left speeds
 * \param right absolute max value for right speeds
 * \return 0 if success, != 0 otherwise
 */
int svs_set_motor1_max_speeds(svs_module_t* module, float left, float right);


/**
 * Set motor2 bank speeds. Speeds are in the range [-1.0(max reverse)..1.0(max forward)]
 * \param left left motor speed
 * \param right right motor speed
 * \param durationms speed command duration in ms
 * \return 0 if success, != 0 otherwise
 */
int svs_set_motor2_speeds(svs_module_t* module, float left, float right, int durationms );

/**
 * Set absolute max speeds for motor2 bank.
 * \param left absolute max value for left speeds
 * \param right absolute max value for right speeds
 * \return 0 if success, != 0 otherwise
 */
int svs_set_motor2_max_speeds(svs_module_t* module, float left, float right);

/**
 * Set servo1 bank values. Servo values are in the range [-1.0(max counter clockwise)..1.0(max clockwise)]
 * \param left left servo value
 * \param right right servo value
 * \return 0 if success, != 0 otherwise
 */
int svs_set_servo1_values(svs_module_t* module, float left, float right );

/**
 * Set absolute max values for servo1 bank.
 * \param left absolute max value for left speeds
 * \param right absolute max value for right speeds
 * \return 0 if success, != 0 otherwise
 */
int svs_set_servo1_max_values(svs_module_t* module, float left, float right);


/**
 * Set servo2 bank values. Servo values are in the range [-1.0(max counter clockwise)..1.0(max clockwise)]
 * \param left left servo value
 * \param right right servo value
 * \return 0 if success, != 0 otherwise
 */
int svs_set_servo2_values(svs_module_t* module, float left, float right );

/**
 * Set absolute max values for servo2 bank.
 * \param left absolute max value for left speeds
 * \param right absolute max value for right speeds
 * \return 0 if success, != 0 otherwise
 */
int svs_set_servo2_max_values(svs_module_t* module, float left, float right);

#ifdef __cplusplus
}
#endif

#endif /* SURVEYORSVS_COMMS_H_ */
